-- This hexapod model demonstrates distributed control, where each leg is controlled by its own script
-- This is meant as an example only. Have a look at the control method of the "ant hexapod", that is much simpler.


-- This is the initialization (only executed once)
if (simGetScriptExecutionCount()==0) then
	baseHandle=simGetObjectHandle('hexa_base')	-- get pointer to the base
	interModuleDelay=4  						-- each leg has a delay of 4 entries in the movement table (offset in sliding window)
	xMovementTable={}  							-- movement data for all directions (sliding window)
	yMovementTable={}
	zMovementTable={}
	tableLength=5*interModuleDelay+1 			-- size of sliding window sufficient to keep movement data for all legs
	for i=1,tableLength,1 do					-- fill table with zeros
		table.insert(xMovementTable,0)
		table.insert(yMovementTable,0)
		table.insert(zMovementTable,0)
	end
	phase=0 									-- phase = movement phase (finite state machine)
	r=0											-- horizontal movement position in step
	z=0											-- vertical movement position in step 
	step=0.005 									-- goal (max) horizontal step size
	vstep=0.002									-- goal (max) vertical step size
	rearExtent=-0.04
	cnt=0										-- expired simulation time
	c=8
	cm0=4
	cm1=16
	prevSt=1

	-- KEEP CONSTANT, no evolution here
	rotation=0 									-- amount the robot rotates around its own axis (-1 to 1)
	direction=0 								-- direction the robot is travelling (0 to math.pi*2)
	forwardVel=1 								-- velocity of movement 
end

-- Following makes the robot first move straight, then in a circle (while keeping its orientation),
--  then again in a circle but this time also rotating.
-- **********************************************************************
cnt=cnt+simGetSimulationTimeStep()

-- Following piece of code is just to adjust for a simulationTimeStep change
-- In that case the table lengths have to be adjusted and the interModule delays, the step sizes, etc.
-- **********************************************************************
st=math.floor((0.05/simGetSimulationTimeStep())+0.5)
if (st~=prevSt) then
	c=c*(st/prevSt)
	cm0=cm0*(st/prevSt)
	cm1=cm1*(st/prevSt)
	interModuleDelay=interModuleDelay*(st/prevSt)
	otx=xMovementTable
	oty=yMovementTable
	otz=zMovementTable
	otl=tableLength
	xMovementTable={}
	yMovementTable={}
	zMovementTable={}
	tableLength=5*interModuleDelay+1
	if ((st/prevSt)>1) then
		for i=1,tableLength,1 do
			table.insert(xMovementTable,otx[math.floor((i/(st/prevSt))+0.51)])
			table.insert(yMovementTable,oty[math.floor((i/(st/prevSt))+0.51)])
			table.insert(zMovementTable,otz[math.floor((i/(st/prevSt))+0.51)])
		end
	else
		for i=1,tableLength,1 do
			table.insert(xMovementTable,otx[math.floor(i*(st/prevSt))])
			table.insert(yMovementTable,oty[math.floor(i*(st/prevSt))])
			table.insert(zMovementTable,otz[math.floor(i*(st/prevSt))])
		end
	end
	prevSt=st
end
-- **********************************************************************

-- Calculate movement data at this time point
if (phase==2) then
	r=r+step*2/st
	z=z-vstep/st
	c=c+1
	if (c>=cm0) then 
		phase=0
		c=0
	end
end
if (phase==1) then
	r=r+step*2/st
	z=z+vstep/st
	c=c+1
	if (c>=cm0) then 
		phase=2
		c=0
	end
end
if (phase==0) then
	r=r-step/st
	c=c+1
	if (c>=cm1) then
		phase=1
		c=0
	end
end

-- convert magnitude r to x-y vector according to set direction (but, we normally only go straight ahead)
x=forwardVel*r*math.cos(direction)
y=forwardVel*r*math.sin(direction)

table.remove(xMovementTable,tableLength)		-- remove last item of FIFOs
table.remove(yMovementTable,tableLength)
table.remove(zMovementTable,tableLength)
table.insert(xMovementTable,1,x)				-- Insert position data in FIFOs
table.insert(yMovementTable,1,y)
table.insert(zMovementTable,1,z)

-- Communicate data (FIFOs) to each of the legs

simSendData(sim_handle_tree,0,'HEXA_x',simPackFloats(xMovementTable))
simSendData(sim_handle_tree,0,'HEXA_y',simPackFloats(yMovementTable))
simSendData(sim_handle_tree,0,'HEXA_z',simPackFloats(zMovementTable))

-- execute child scripts
simHandleChildScript(sim_handle_all_except_explicit,baseHandle,interModuleDelay,rotation)
